{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "from mpmath import atan2, degrees"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_hypotenuse(a, b):\n",
    "  return sqrt(a*a + b*b)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_cosine_law_angle(a, b, c):\n",
    "    \n",
    "  cos_gamma = (a*a + b*b - c*c) / (2*a*b)\n",
    "  sin_gamma = sqrt(1 - cos_gamma * cos_gamma)\n",
    "  gamma = atan2(sin_gamma, cos_gamma)\n",
    "\n",
    "  return gamma"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2.04691538770669\n",
      "1.5707963267949\n"
     ]
    }
   ],
   "source": [
    "# Test cosine function \n",
    "\n",
    "g = get_cosine_law_angle(3, 4, 6)\n",
    "print(g) # g = 2.05\n",
    "\n",
    "g = get_cosine_law_angle(3, 4, 5)\n",
    "print(g) # g = 1.57"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "5\n",
      "sqrt(34)\n",
      "1.50097168527591\n",
      "1.53481186671284\n"
     ]
    }
   ],
   "source": [
    "# Test hypotenuse function \n",
    "\n",
    "c = get_hypotenuse(3, 4)\n",
    "print(c) # 5\n",
    "\n",
    "c = get_hypotenuse(3, 5)\n",
    "print(c) # sqrt(34) = 0.53\n",
    "\n",
    "l = get_hypotenuse( 1.5, 0.054)\n",
    "print(l)\n",
    "\n",
    "gamma = atan2(1.5, 0.054)\n",
    "print(gamma)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_first_three_angles(wrist_center):\n",
    "  # given the wrist center which a tuple of 3 numbers x, y, z\n",
    "  # (x, y, z) is the wrist center point wrt base frame\n",
    "  # return the angles q1, q2, q3 for each respective joint\n",
    "  # given geometry of the kuka kr210\n",
    "    \n",
    "  x, y, z  = wrist_center\n",
    "    \n",
    "  a1, a2, a3 = 0.35, 1.25, -0.054\n",
    "  d1, d4 = 0.75, 1.5\n",
    "  l = 1.50097168527591 #get_hypotenuse(d4, -a3)\n",
    "  phi = 1.53481186671284 # atan2(d4, -a3)\n",
    "  \n",
    "  x_prime = get_hypotenuse(x, y)\n",
    "  mx = x_prime -  a1\n",
    "  my = z - d1 \n",
    "  m = get_hypotenuse(mx, my)\n",
    "  alpha = atan2(my, mx)\n",
    "  \n",
    "  gamma = get_cosine_law_angle(l, a2, m)\n",
    "  beta = get_cosine_law_angle(m, a2, l)\n",
    "  \n",
    "  q1 = atan2(y, x)\n",
    "  q2 = pi/2 - beta - alpha \n",
    "  q3 = -(gamma - phi)\n",
    "    \n",
    "  return q1, q2, q3 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "gamma 1.93479351084403\n",
      "phi 1.53481186671284\n",
      "\n",
      " q1: 0.799999272099671 \n",
      " q2: -0.470820255901082 + pi/2 \n",
      " q3: -0.399981644131188\n",
      "1.09997607089381\n"
     ]
    }
   ],
   "source": [
    "# Test wrist center inverse kinematics\n",
    "wrist_center = 1.79505, 1.84825, 0.3094\n",
    "\n",
    "q1, q2, q3 = get_first_three_angles(wrist_center)\n",
    "print(\"\\n q1:\", q1, \"\\n q2:\", q2, \"\\n q3:\", q3)# q1 = 0.8, q2 = 1.1, q3 = -0.4\n",
    "\n",
    "print(q2.evalf())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "q2: -0.2770352502003999\n",
      "q3: -0.11681192184062983\n",
      "gamma 1.65003113496368\n",
      "phi 1.53481186671284\n",
      "\n",
      " q1: 1.01227245775763 \n",
      " q2: -1.8470352502004 + pi/2 \n",
      " q3: -0.115219268250835\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "-0.276238923405500"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "print(\"q2:\", -1.8470352502004 + 3.14 / 2 )\n",
    "print(\"q3:\", -3.25681192184063 + 3.14)\n",
    "\n",
    "wrist_center = 0.750499, 1.201, 2.475\n",
    "\n",
    "q1, q2, q3 = get_first_three_angles(wrist_center)\n",
    "print(\"\\n q1:\", q1, \"\\n q2:\", q2, \"\\n q3:\", q3)\n",
    "# q1 = 1.012, q2 = -0.2758003637377226 , q3 = -0.11568\n",
    "\n",
    "q2.evalf()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
